{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# TriangulationFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`TriangulationFactor<CAMERA>` is a unary factor that constrains a single unknown 3D point (`Point3`) based on a 2D measurement from a **known** camera.\n",
        "It's essentially the inverse of a projection factor where the camera pose and calibration are fixed, and only the 3D point is variable.\n",
        "\n",
        "Key characteristics:\n",
        "- **Unary Factor:** Connects only to a `Point3` variable key.\n",
        "- **Known Camera:** The `CAMERA` object (e.g., `PinholeCameraCal3_S2`, `StereoCamera`) containing the fixed pose and calibration is provided during factor construction.\n",
        "- **Measurement:** Takes a 2D measurement (`Point2` for monocular, `StereoPoint2` for stereo).\n",
        "- **Error:** Calculates the reprojection error: $ \\text{error}(L) = \\text{camera.project}(L) - z $\n",
        "\n",
        "**Use Case:** Useful in triangulation scenarios where multiple camera views with known poses observe an unknown landmark. A `NonlinearFactorGraph` containing only `TriangulationFactor`s (one for each view) can be optimized to find the maximum likelihood estimate of the 3D landmark position."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/TriangulationFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Point3, Point2, Pose3, Rot3, Cal3_S2, PinholeCameraCal3_S2, Values, NonlinearFactorGraph\n",
        "# The Python wrapper often creates specific instantiations\n",
        "from gtsam import TriangulationFactorCal3_S2\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "L = symbol_shorthand.L"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating a TriangulationFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "Instantiate by providing:\n",
        "1. The known `CAMERA` object.\n",
        "2. The 2D measurement.\n",
        "3. The noise model for the measurement.\n",
        "4. The key for the unknown `Point3` landmark.\n",
        "5. (Optional) Cheirality handling flags."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "TriangulationFactor: TriangulationFactor,camera.pose R: [\n",
            "\t0.990033, -0.117578, -0.0775207;\n",
            "\t0.0993347, 0.97319, -0.207445;\n",
            "\t0.0998334, 0.197677, 0.97517\n",
            "]\n",
            "t:   1   0 0.5\n",
            "camera.calibration[\n",
            "\t500, 0, 320;\n",
            "\t0, 500, 240;\n",
            "\t0, 0, 1\n",
            "]\n",
            "z[\n",
            "\t450.5;\n",
            "\t300.2\n",
            "]\n",
            "  keys = { l0 }\n",
            "  noise model: unit (2) \n"
          ]
        }
      ],
      "source": [
        "# Known camera parameters\n",
        "K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n",
        "pose = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(1, 0, 0.5))\n",
        "camera = PinholeCameraCal3_S2(pose, K)\n",
        "\n",
        "# Measurement observed by this camera\n",
        "measured_pt2 = Point2(450.5, 300.2)\n",
        "pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
        "landmark_key = L(0)\n",
        "\n",
        "# Factor type includes the Camera type\n",
        "factor = TriangulationFactorCal3_S2(camera, measured_pt2, pixel_noise, landmark_key)\n",
        "factor.print(\"TriangulationFactor: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The error is the reprojection error given an estimate of the landmark's `Point3` position."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Reprojection error for estimate [3.  0.5 2. ]: 314012.75623020524\n",
            "Expected projection: [1225.10768109  467.55726116]\n",
            "Manual error calculation: [774.60768109 167.35726116]\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "\n",
        "# Estimate for the landmark position\n",
        "landmark_estimate = Point3(3.0, 0.5, 2.0)\n",
        "values.insert(landmark_key, landmark_estimate)\n",
        "\n",
        "error = factor.error(values)\n",
        "print(f\"Reprojection error for estimate {landmark_estimate}: {error}\")\n",
        "\n",
        "# Calculate expected projection\n",
        "expected_projection = camera.project(landmark_estimate)\n",
        "manual_error = expected_projection - measured_pt2\n",
        "print(f\"Expected projection: {expected_projection}\")\n",
        "print(f\"Manual error calculation: {manual_error}\")\n",
        "assert np.allclose(factor.unwhitenedError(values), manual_error)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "usage_header_md"
      },
      "source": [
        "## Usage in Triangulation"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "usage_desc_md"
      },
      "source": [
        "Multiple `TriangulationFactor`s, one for each known camera view, can be added to a graph to solve for the landmark position."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "usage_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "\n",
            "Optimized Landmark Position:\n",
            "Values with 1 values:\n",
            "Value l0: (class Eigen::Matrix<double,-1,1,0,-1,1>)\n",
            "[\n",
            "\t-12446.1;\n",
            "\t-55075.8;\n",
            "\t2.39319e+06\n",
            "]\n",
            "\n",
            "Final Error: 7855.8598\n"
          ]
        }
      ],
      "source": [
        "# Create a second camera and measurement\n",
        "pose2 = Pose3(Rot3.Ypr(-0.1, 0.1, -0.1), Point3(-1, 0, 0.5))\n",
        "camera2 = PinholeCameraCal3_S2(pose2, K)\n",
        "measured_pt2_cam2 = Point2(180.0, 190.0)\n",
        "factor2 = TriangulationFactorCal3_S2(camera2, measured_pt2_cam2, pixel_noise, landmark_key)\n",
        "\n",
        "# Create graph and add factors\n",
        "triangulation_graph = NonlinearFactorGraph()\n",
        "triangulation_graph.add(factor)\n",
        "triangulation_graph.add(factor2)\n",
        "\n",
        "# Optimize (requires an initial estimate)\n",
        "initial_estimate = Values()\n",
        "initial_estimate.insert(landmark_key, Point3(2, 0, 5)) # Initial guess\n",
        "\n",
        "optimizer = gtsam.LevenbergMarquardtOptimizer(triangulation_graph, initial_estimate)\n",
        "result = optimizer.optimize()\n",
        "\n",
        "print(\"\\nOptimized Landmark Position:\")\n",
        "result.print()\n",
        "print(f\"Final Error: {triangulation_graph.error(result):.4f}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
